<pre class="code">
<span class="srcline"><span class="lineno"><a href="45,1" id="srcline1">  1</a></span><span class="line"><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,2" id="srcline2">  2</a></span><span class="line"><span class="comment">% RPI-MATLAB-Simulator</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,3" id="srcline3">  3</a></span><span class="line"><span class="comment">% http://code.google.com/p/rpi-matlab-simulator/</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,4" id="srcline4">  4</a></span><span class="line"><span class="comment">% preDynamics.m</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,5" id="srcline5">  5</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,6" id="srcline6">  6</a></span><span class="line"><span class="comment">% Generates submatrices that are common for dynamics formulations including</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,7" id="srcline7">  7</a></span><span class="line"><span class="comment">%   M   - Mass-inertia</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,8" id="srcline8">  8</a></span><span class="line"><span class="comment">%   Gn  - Contact normal wrench</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,9" id="srcline9">  9</a></span><span class="line"><span class="comment">%   Gf  - Contact friction wrench</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,10" id="srcline10"> 10</a></span><span class="line"><span class="comment">%   Gb  - Bilateral constraint wrench</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,11" id="srcline11"> 11</a></span><span class="line"><span class="comment">%   E   - Mask matrix for friction</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,12" id="srcline12"> 12</a></span><span class="line"><span class="comment">%   U   - Coefficients of friction</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,13" id="srcline13"> 13</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,14" id="srcline14"> 14</a></span><span class="line"><span class="comment">%   NU  - Velocities, including rotational           These 3 are vectors</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,15" id="srcline15"> 15</a></span><span class="line"><span class="comment">%   FX  - External forces, including rotational</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,16" id="srcline16"> 16</a></span><span class="line"><span class="comment">%   PSI - Gap distances </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,17" id="srcline17"> 17</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="45,18" id="srcline18"> 18</a></span><span class="line"><span class="keyword">function</span> <span class="var type1" id="S2T6U3">sim</span> = preDynamics( <span class="mxinfo" id="T6:U2"><span class="mxinfo" id="T6:U3"><span class="var type1" id="S2T6U6">sim</span></span></span> )</span></span>
<span class="srcline"><span class="lineno"><a href="45,19" id="srcline19"> 19</a></span><span class="line">  <span class="comment">%#codegen</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,20" id="srcline20"> 20</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="45,21" id="srcline21"> 21</a></span><span class="line">  <span class="comment">% We need to add joint bodies to the set of activeBodies.  Further, by</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,22" id="srcline22"> 22</a></span><span class="line">  <span class="comment">% doing this first, we ensure that the mass-inertia matrix contains joint</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,23" id="srcline23"> 23</a></span><span class="line">  <span class="comment">% bodies first.  This is useful in postDynamics when applying joint corrections.  </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,24" id="srcline24"> 24</a></span><span class="line">  <span class="mxinfo" id="T1:U5"><span class="var type1" id="S3T1U9">nj</span> = <span class="mxinfo" id="T1:U7">length(<span class="var type1" id="S2T6U13">sim</span>.joints)</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="45,25" id="srcline25"> 25</a></span><span class="line">  <span class="keyword">for</span> <span class="mxinfo" id="T1:U9"><span class="var type1" id="S5T1U17">j</span></span>=1:<span class="var type1" id="S3T1U20">nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,26" id="srcline26"> 26</a></span><span class="line">     <span class="var type2" id="S2T0U23">sim</span> = sim_activateBodies( <span class="var type2" id="S2T0U26">sim</span>, <span class="var type2" id="S2T0U30">sim</span>.joints(<span class="var type2" id="S5T0U32">j</span>).body1id, <span class="var type2" id="S2T0U37">sim</span>.joints(<span class="var type2" id="S5T0U39">j</span>).body2id );</span></span>
<span class="srcline"><span class="lineno"><a href="45,27" id="srcline27"> 27</a></span><span class="line">  <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,28" id="srcline28"> 28</a></span><span class="line">  <span class="mxinfo" id="T1:U12"><span class="var type1" id="S7T1U43">njc</span> = <span class="mxinfo" id="T1:U14"><span class="var type1" id="S2T6U45">sim</span>.num_jointConstraints</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="45,29" id="srcline29"> 29</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="45,30" id="srcline30"> 30</a></span><span class="line">  <span class="comment">%% Useful vars </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,31" id="srcline31"> 31</a></span><span class="line">  <span class="mxinfo" id="T1:U16"><span class="var type1" id="S8T1U49">nb</span> = <span class="mxinfo" id="T1:U18"><span class="var type1" id="S2T6U51">sim</span>.num_activeBodies</span></span>;    <span class="comment">% Number of bodies with contacts</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,32" id="srcline32"> 32</a></span><span class="line">  <span class="mxinfo" id="T1:U20"><span class="var type1" id="S9T1U55">nc</span> = <span class="mxinfo" id="T1:U22">length(<span class="var type1" id="S2T6U59">sim</span>.contacts)</span></span>;    <span class="comment">% Number of contacts</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,33" id="srcline33"> 33</a></span><span class="line">  <span class="mxinfo" id="T1:U24"><span class="var type1" id="S10T1U63">nd</span> = <span class="mxinfo" id="T1:U26"><span class="var type1" id="S2T6U65">sim</span>.num_fricdirs</span></span>;        <span class="comment">% Number of directions in discrete friction "cone"</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,34" id="srcline34"> 34</a></span><span class="line">  </span></span>
<span class="srcline"><span class="lineno"><a href="45,35" id="srcline35"> 35</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="45,36" id="srcline36"> 36</a></span><span class="line">  <span class="comment">%% Init submatrices </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,37" id="srcline37"> 37</a></span><span class="line">  <span class="mxinfo" id="T38:U28"><span class="var type1" id="S11T38U69">M</span> = <span class="mxinfo" id="T38:U30">zeros(<span class="mxinfo" id="T1:U31"><span class="mxinfo" id="T1:U32">6</span>*<span class="var type1" id="S8T1U74">nb</span></span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="45,38" id="srcline38"> 38</a></span><span class="line">  <span class="mxinfo" id="T38:U34"><span class="var type1" id="S13T38U77">Gn</span> = <span class="mxinfo" id="T38:U36">zeros(<span class="mxinfo" id="T1:U37"><span class="mxinfo" id="T1:U38">6</span>*<span class="var type1" id="S8T1U82">nb</span></span>,<span class="mxinfo" id="T1:U40"><span class="var type1" id="S9T1U83">nc</span></span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="45,39" id="srcline39"> 39</a></span><span class="line">  <span class="keyword">if</span> <span class="mxinfo" id="T4:U42"><span class="var type1" id="S2T6U87">sim</span>.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,40" id="srcline40"> 40</a></span><span class="line">      <span class="mxinfo" id="T38:U44"><span class="var type1" id="S14T38U91">Gf</span> = <span class="mxinfo" id="T38:U46">zeros(<span class="mxinfo" id="T1:U47"><span class="mxinfo" id="T1:U48">6</span>*<span class="var type1" id="S8T1U96">nb</span></span>,<span class="mxinfo" id="T1:U50"><span class="var type1" id="S10T1U98">nd</span>*<span class="mxinfo" id="T1:U52"><span class="var type1" id="S9T1U99">nc</span></span></span>)</span></span>;  </span></span>
<span class="srcline"><span class="lineno"><a href="45,41" id="srcline41"> 41</a></span><span class="line">      <span class="mxinfo" id="T9:U54"><span class="var type1" id="S15T9U102">U</span> = <span class="mxinfo" id="T9:U56">zeros(<span class="mxinfo" id="T1:U57"><span class="var type1" id="S9T1U105">nc</span></span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="45,42" id="srcline42"> 42</a></span><span class="line">      <span class="mxinfo" id="T38:U59"><span class="var type1" id="S16T38U108">E</span> = <span class="mxinfo" id="T38:U61">zeros(<span class="mxinfo" id="T1:U62"><span class="var type1" id="S10T1U112">nd</span>*<span class="mxinfo" id="T1:U64"><span class="var type1" id="S9T1U113">nc</span></span></span>,<span class="mxinfo" id="T1:U66"><span class="var type1" id="S9T1U114">nc</span></span>)</span></span>;  <span class="comment">% TODO: joint friction</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,43" id="srcline43"> 43</a></span><span class="line">  <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,44" id="srcline44"> 44</a></span><span class="line">  <span class="mxinfo" id="T38:U68"><span class="var type1" id="S17T38U117">Gb</span> = <span class="mxinfo" id="T38:U70">zeros(<span class="mxinfo" id="T1:U71"><span class="mxinfo" id="T1:U72">6</span>*<span class="var type1" id="S8T1U122">nb</span></span>,<span class="var type1" id="S7T1U123">njc</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="45,45" id="srcline45"> 45</a></span><span class="line">  <span class="mxinfo" id="T39:U75"><span class="var type1" id="S18T39U126">NU</span> = <span class="mxinfo" id="T39:U77">zeros(<span class="mxinfo" id="T1:U78"><span class="mxinfo" id="T1:U79">6</span>*<span class="var type1" id="S8T1U131">nb</span></span>,1)</span></span>;   <span class="comment">% Velocity, including angular   </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,46" id="srcline46"> 46</a></span><span class="line">  <span class="mxinfo" id="T39:U81"><span class="var type1" id="S19T39U135">FX</span> = <span class="mxinfo" id="T39:U83">zeros(<span class="mxinfo" id="T1:U84"><span class="mxinfo" id="T1:U85">6</span>*<span class="var type1" id="S8T1U140">nb</span></span>,1)</span></span>;   <span class="comment">% External force (not impulse!) </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,47" id="srcline47"> 47</a></span><span class="line">  <span class="mxinfo" id="T31:U87"><span class="var type1" id="S20T31U144">PSI</span> = <span class="mxinfo" id="T31:U89">zeros(<span class="mxinfo" id="T1:U90"><span class="var type1" id="S9T1U147">nc</span></span>,1)</span></span>;    <span class="comment">% Gap distance per contact, psi_n  </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,48" id="srcline48"> 48</a></span><span class="line">  <span class="comment">%sim.num_subContacts = 0; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,49" id="srcline49"> 49</a></span><span class="line">  </span></span>
<span class="srcline"><span class="lineno"><a href="45,50" id="srcline50"> 50</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="45,51" id="srcline51"> 51</a></span><span class="line">  <span class="comment">%% Calculate submatrices</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,52" id="srcline52"> 52</a></span><span class="line">  </span></span>
<span class="srcline"><span class="lineno"><a href="45,53" id="srcline53"> 53</a></span><span class="line">  <span class="comment">% M, NU, and FX</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,54" id="srcline54"> 54</a></span><span class="line"><span class="comment">%   for b=1:length(sim.bodies)</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,55" id="srcline55"> 55</a></span><span class="line"><span class="comment">%      B = sim.bodies(b);</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,56" id="srcline56"> 56</a></span><span class="line"><span class="comment">%      if B.active &amp;&amp; B.dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,57" id="srcline57"> 57</a></span><span class="line"><span class="comment">%         cID = B.bodyContactID;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,58" id="srcline58"> 58</a></span><span class="line"><span class="comment">%         M(6*cID-5:6*cID,6*cID-5:6*cID) = body_massInertiaMatrix(B);  </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,59" id="srcline59"> 59</a></span><span class="line"><span class="comment">%         NU(6*cID-5:6*cID) = B.nu;                  % NU</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,60" id="srcline60"> 60</a></span><span class="line"><span class="comment">%         FX(6*cID-5:6*cID) = B.Fext;                % FX</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,61" id="srcline61"> 61</a></span><span class="line"><span class="comment">%      end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,62" id="srcline62"> 62</a></span><span class="line"><span class="comment">%   end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,63" id="srcline63"> 63</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,64" id="srcline64"> 64</a></span><span class="line"><span class="comment">%   % Gn, E, U, and Gf</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,65" id="srcline65"> 65</a></span><span class="line"><span class="comment">%   for cID=1:nc</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,66" id="srcline66"> 66</a></span><span class="line"><span class="comment">%       </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,67" id="srcline67"> 67</a></span><span class="line"><span class="comment">%     C = sim.contacts(cID);</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,68" id="srcline68"> 68</a></span><span class="line"><span class="comment">%     sim.num_subContacts = sim.num_subContacts + length(C.psi_n);    % TODO: count the number of subcontacts</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,69" id="srcline69"> 69</a></span><span class="line"><span class="comment">%     </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,70" id="srcline70"> 70</a></span><span class="line"><span class="comment">%     PSI(cID) = C.psi_n(1);                                          % PSI</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,71" id="srcline71"> 71</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,72" id="srcline72"> 72</a></span><span class="line"><span class="comment">%     % For every contact, there are two bodies which enter Gn and Gf </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,73" id="srcline73"> 73</a></span><span class="line"><span class="comment">%     % (unless contact was with a static body). </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,74" id="srcline74"> 74</a></span><span class="line"><span class="comment">%     </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,75" id="srcline75"> 75</a></span><span class="line"><span class="comment">%     % Body 1</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,76" id="srcline76"> 76</a></span><span class="line"><span class="comment">%     B1 = sim.bodies(C.body1_id);</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,77" id="srcline77"> 77</a></span><span class="line"><span class="comment">%     if B1.dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,78" id="srcline78"> 78</a></span><span class="line"><span class="comment">%         r1 = C.p1' - B1.u;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,79" id="srcline79"> 79</a></span><span class="line"><span class="comment">%         body1id = B1.bodyContactID;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,80" id="srcline80"> 80</a></span><span class="line"><span class="comment">%         Gn_i1 = [-C.normal(1,:)'; cross3(r1,-C.normal(1,:)')];</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,81" id="srcline81"> 81</a></span><span class="line"><span class="comment">%         Gn(6*body1id-5:6*body1id,cID) = Gn_i1;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,82" id="srcline82"> 82</a></span><span class="line"><span class="comment">%     end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,83" id="srcline83"> 83</a></span><span class="line"><span class="comment">%     % Body 2</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,84" id="srcline84"> 84</a></span><span class="line"><span class="comment">%     B2 = sim.bodies(C.body2_id); </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,85" id="srcline85"> 85</a></span><span class="line"><span class="comment">%     if sim.bodies(C.body2_id).dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,86" id="srcline86"> 86</a></span><span class="line"><span class="comment">%         r2 = (C.p1+C.normal*C.psi_n)' - B2.u;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,87" id="srcline87"> 87</a></span><span class="line"><span class="comment">%         body2id = B2.bodyContactID;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,88" id="srcline88"> 88</a></span><span class="line"><span class="comment">%         Gn_i2 = [C.normal(1,:)'; cross3(r2,C.normal(1,:)')];</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,89" id="srcline89"> 89</a></span><span class="line"><span class="comment">%         Gn(6*body2id-5:6*body2id,cID) = Gn_i2;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,90" id="srcline90"> 90</a></span><span class="line"><span class="comment">%     end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,91" id="srcline91"> 91</a></span><span class="line"><span class="comment">%     </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,92" id="srcline92"> 92</a></span><span class="line"><span class="comment">%     if sim.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,93" id="srcline93"> 93</a></span><span class="line"><span class="comment">%         % E </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,94" id="srcline94"> 94</a></span><span class="line"><span class="comment">%         E(nd*cID-(nd-1):nd*cID,cID) = ones(nd,1);   </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,95" id="srcline95"> 95</a></span><span class="line"><span class="comment">%         % U</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,96" id="srcline96"> 96</a></span><span class="line"><span class="comment">%         U(cID,cID) = 0.5*B1.mu * B2.mu;     </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,97" id="srcline97"> 97</a></span><span class="line"><span class="comment">%         % Gf</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,98" id="srcline98"> 98</a></span><span class="line"><span class="comment">%         % TODO: Select the initial tangent opposing motion</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,99" id="srcline99"> 99</a></span><span class="line"><span class="comment">%         randTan = arbitraryTangent(C.normal(1,:)); </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,100" id="srcline100">100</a></span><span class="line"><span class="comment">%         for j=1:nd</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,101" id="srcline101">101</a></span><span class="line"><span class="comment">%             d = rot(C.normal(1,:),((j-1)/nd)*(2*pi)) * randTan;    % Friction direction d</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,102" id="srcline102">102</a></span><span class="line"><span class="comment">%             if B1.dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,103" id="srcline103">103</a></span><span class="line"><span class="comment">%                 Gf(6*body1id-5:6*body1id, nd*(cID-1)+j) = [d; cross3(r1,d)];</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,104" id="srcline104">104</a></span><span class="line"><span class="comment">%             end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,105" id="srcline105">105</a></span><span class="line"><span class="comment">%             if B2.dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,106" id="srcline106">106</a></span><span class="line"><span class="comment">%                 Gf(6*body2id-5:6*body2id, nd*(cID-1)+j) = [d; cross3(r2,d)];</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,107" id="srcline107">107</a></span><span class="line"><span class="comment">%             end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,108" id="srcline108">108</a></span><span class="line"><span class="comment">%         end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,109" id="srcline109">109</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,110" id="srcline110">110</a></span><span class="line"><span class="comment">%     end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,111" id="srcline111">111</a></span><span class="line"><span class="comment">%   end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,112" id="srcline112">112</a></span><span class="line"><span class="comment">%   </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,113" id="srcline113">113</a></span><span class="line"><span class="comment">%   %% Joint Dynamics </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,114" id="srcline114">114</a></span><span class="line"><span class="comment">%   joint_bn = zeros(njc, 1);</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,115" id="srcline115">115</a></span><span class="line"><span class="comment">%     </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,116" id="srcline116">116</a></span><span class="line"><span class="comment">%   for j=1:nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,117" id="srcline117">117</a></span><span class="line"><span class="comment">%      sim = updateJoint( sim, j );</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,118" id="srcline118">118</a></span><span class="line"><span class="comment">%      Jnt = sim.joints(j); </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,119" id="srcline119">119</a></span><span class="line"><span class="comment">%      b1id = sim.bodies(Jnt.body1id).bodyContactID;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,120" id="srcline120">120</a></span><span class="line"><span class="comment">%      b2id = sim.bodies(Jnt.body2id).bodyContactID;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,121" id="srcline121">121</a></span><span class="line"><span class="comment">%      constIndex = Jnt.constraintIndex; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,122" id="srcline122">122</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,123" id="srcline123">123</a></span><span class="line"><span class="comment">%      [G1c G2c] = joint_Jacobians(sim,j);</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,124" id="srcline124">124</a></span><span class="line"><span class="comment">%      [C Cdot] = joint_constraintError(sim,j);   % TODO: This is probably not necessary.</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,125" id="srcline125">125</a></span><span class="line"><span class="comment">%      %[C Cdot] = Jnt.constraintError();</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,126" id="srcline126">126</a></span><span class="line"><span class="comment">%      %bj = 0.0*C/sim.h + Cdot;    </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,127" id="srcline127">127</a></span><span class="line"><span class="comment">%      bj = 0.9*Cdot;  % Maybe add a coefficient 0&lt;=C&lt;=1 for stability.</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,128" id="srcline128">128</a></span><span class="line"><span class="comment">%      </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,129" id="srcline129">129</a></span><span class="line"><span class="comment">%      if sim.bodies(Jnt.body1id).dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,130" id="srcline130">130</a></span><span class="line"><span class="comment">%         Gb(6*b1id-5:6*b1id, constIndex) = G1c; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,131" id="srcline131">131</a></span><span class="line"><span class="comment">%         %if sim.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,132" id="srcline132">132</a></span><span class="line"><span class="comment">%             %Gf(6*b1id-5:6*b1id, nd*nc+j) = G1f;   % TODO: not yet implemented</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,133" id="srcline133">133</a></span><span class="line"><span class="comment">%         %end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,134" id="srcline134">134</a></span><span class="line"><span class="comment">%      end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,135" id="srcline135">135</a></span><span class="line"><span class="comment">%      if sim.bodies(Jnt.body2id).dynamic</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,136" id="srcline136">136</a></span><span class="line"><span class="comment">%         Gb(6*b2id-5:6*b2id, constIndex) = G2c;  % TODO: fix indices </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,137" id="srcline137">137</a></span><span class="line"><span class="comment">%         %if sim.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,138" id="srcline138">138</a></span><span class="line"><span class="comment">%             %Gf(6*b2id-5:6*b2id, nd*nc+j) = G2f; % TODO: this is wrong now</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,139" id="srcline139">139</a></span><span class="line"><span class="comment">%         %end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,140" id="srcline140">140</a></span><span class="line"><span class="comment">%      end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,141" id="srcline141">141</a></span><span class="line"><span class="comment">%      </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,142" id="srcline142">142</a></span><span class="line"><span class="comment">%      if sim.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,143" id="srcline143">143</a></span><span class="line"><span class="comment">%          % TODO: joint friction</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,144" id="srcline144">144</a></span><span class="line"><span class="comment">%      end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,145" id="srcline145">145</a></span><span class="line"><span class="comment">%      </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,146" id="srcline146">146</a></span><span class="line"><span class="comment">%      % Store joint constraint errors for vector b</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,147" id="srcline147">147</a></span><span class="line"><span class="comment">%      joint_bn(constIndex,1) = bj;  </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,148" id="srcline148">148</a></span><span class="line"><span class="comment">% </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,149" id="srcline149">149</a></span><span class="line"><span class="comment">%   end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,150" id="srcline150">150</a></span><span class="line"><span class="comment">%   sim.dynamics.joint_bn = joint_bn; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,151" id="srcline151">151</a></span><span class="line"><span class="comment">%   </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,152" id="srcline152">152</a></span><span class="line"><span class="comment">%   </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,153" id="srcline153">153</a></span><span class="line"><span class="comment">%   %% Store values in the struct sim.dynamics  </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,154" id="srcline154">154</a></span><span class="line"><span class="comment">%   sim.dynamics.M = M;        % It seems that assignment here is faster than</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,155" id="srcline155">155</a></span><span class="line"><span class="comment">%   sim.dynamics.Gn = Gn;      % referencing the struct multiple times above.</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,156" id="srcline156">156</a></span><span class="line"><span class="comment">%   sim.dynamics.Gb = Gb; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,157" id="srcline157">157</a></span><span class="line"><span class="comment">%   if sim.FRICTION</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,158" id="srcline158">158</a></span><span class="line"><span class="comment">%       sim.dynamics.Gf = Gf;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,159" id="srcline159">159</a></span><span class="line"><span class="comment">%       sim.dynamics.U = U;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,160" id="srcline160">160</a></span><span class="line"><span class="comment">%       sim.dynamics.E = E; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,161" id="srcline161">161</a></span><span class="line"><span class="comment">%   end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,162" id="srcline162">162</a></span><span class="line"><span class="comment">%   sim.dynamics.NU = NU;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,163" id="srcline163">163</a></span><span class="line"><span class="comment">%   sim.dynamics.FX = FX;</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,164" id="srcline164">164</a></span><span class="line"><span class="comment">%   sim.dynamics.PSI = PSI; </span></span></span>
<span class="srcline"><span class="lineno"><a href="45,165" id="srcline165">165</a></span><span class="line">  </span></span>
<span class="srcline"><span class="lineno"><a href="45,166" id="srcline166">166</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="45,167" id="srcline167">167</a></span><span class="line"></span></span>
</pre>
